#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <qwt_slider.h>
#include <QBoxLayout>
#include <iostream>

#define pi_define 3.141592654

MainWindow::MainWindow(QWidget *parent) :

    QMainWindow(parent)
{
        end_program_button = new QPushButton(tr("End Program"), this);
        end_program_button->setFont(QFont("Times", 12, QFont::Bold));
        end_program_button->setStyleSheet("*{border: 1px solid  #09A72E; border-radius: 4px;background-color: rgb(98,246,135)}");
        QRect pos_end_program_button = QRect(0, 0, 120, 20);
        end_program_button->setGeometry(pos_end_program_button);
        connect(end_program_button, SIGNAL(clicked()), this, SLOT(end_program()));

        float DC_0_kp_min = 0.0;
        float DC_0_kp_max = 500.0;
        float DC_0_kp_inc = 0.001;
        float DC_0_kp_init = 2;

        float DC_0_kv_min = 0.0;
        float DC_0_kv_max = 500.0;
        float DC_0_kv_inc = 0.001;
        float DC_0_kv_init = 20;

        float DC_0_ki_min = 0.0;
        float DC_0_ki_max = 2.0;
        float DC_0_ki_inc = 0.0001;
        float DC_0_ki_init = 0.005;

        float DC_0_pos_min = -2000;
        float DC_0_pos_max = 2000;
        float DC_0_pos_inc = 1;
        float DC_0_pos_init = 0;

        float DC_0_vel_min = -100;
        float DC_0_vel_max = 100;
        float DC_0_vel_inc = 1;
        float DC_0_vel_init = 0;

        float DC_0_accel_min = 0;
        float DC_0_accel_max = 100;
        float DC_0_accel_inc = 1;
        float DC_0_accel_init = DC_0_accel_max;
        //float DC_0_accel_init = DC_0_accel_max;

        float DC_0_CPR = 360.0;

        DC_motor_0 = new my_DC_motor(this, "DC 0", 0, DC_0_CPR, DC_0_kp_min, DC_0_kp_max, DC_0_kp_inc, DC_0_kp_init, DC_0_kv_min, DC_0_kv_max, DC_0_kv_inc, DC_0_kv_init, DC_0_ki_min, DC_0_ki_max, DC_0_ki_inc, DC_0_ki_init, DC_0_pos_min, DC_0_pos_max, DC_0_pos_inc, DC_0_pos_init, DC_0_vel_min, DC_0_vel_max, DC_0_vel_inc, DC_0_vel_init, DC_0_accel_min, DC_0_accel_max, DC_0_accel_inc, DC_0_accel_max);
        DC_motor_0->setGeometry(QRect(900,0,410,600));



        /*
        servo_start_angle.push_back(10);
        servo_start_angle.push_back(20);
        servo_start_angle.push_back(30);
        servo_start_angle.push_back(40);
        servo_board = new my_servo(this, servo_start_angle);
        servo_board->setGeometry(QRect(0,600,1600,500));
        */

        /*
        slider_test = new slider_with_box(this, "Test", 0, -10000, 10000, 1, 0);
        slider_test->setGeometry(QRect(0, 200, 380, 60));
        */

        start_or_end_program_flag = 1;
        gui_timer_id = startTimer(33);

        PID_Timer = new QTimer(this);
        // Set PID_Loop() function to run at 100Hz rate and start looping
        PID_Timer->setInterval(1);
        connect(PID_Timer, SIGNAL(timeout()), this, SLOT(PID_Loop()));
        PID_Timer->start();
}

void MainWindow::PID_Loop()
{
    // Update the PID control law for position control of DC motor
    DC_motor_0->move_motor();
}

MainWindow::~MainWindow()
{

}

void MainWindow::timerEvent(QTimerEvent *event)
{
        if(start_or_end_program_flag != -1)
        {
         //stepper_Rx_mutex->lock();
            DC_motor_0->timer_update(event);
            //servo_board->timer_update(event);

         //stepper_Rx_mutex->unlock();

        }
        else if(start_or_end_program_flag == -1)
        {
                killTimer(gui_timer_id);
                qApp->closeAllWindows(); //must be called from gui thread
        }
}


void MainWindow::end_program()
{
        start_or_end_program_flag = -1;
        DC_motor_0->close();
        //NOT WORKING servo_board->close();
}

void MainWindow::start_program()
{
        start_or_end_program_flag = 1;
}
